package rescue;

import static java.awt.Color.BLUE;
import static java.awt.Color.CYAN;
import static java.awt.Color.GREEN;
import static java.awt.Color.MAGENTA;
import static java.awt.Color.ORANGE;
import static java.awt.Color.RED;
import static java.awt.Color.WHITE;
import static java.awt.Color.YELLOW;

import java.awt.Color;
import java.awt.image.BufferedImage;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashSet;
import java.util.Set;

import javax.vecmath.Vector3d;

import simbad.sim.RescueRobot;


public class MyRescueRobot extends RescueRobot
{
    private final double size = 10;
    private BufferedImage colourImage = null;
    private int imageWidth;
    private int imageHeight;

    // My attributes
    private final ArrayList<Color> OBJECT_COLORS = new ArrayList<Color>();
    private Behaviour currentGoal = Behaviour.SEARCH;
    
    // My constants
    private enum Behaviour {SEARCH, OBJECT, AVOID};
    private final int wallClearance = 2;
        
    public MyRescueRobot(Vector3d position, int id, String name)
    {
	super(position, id, name);

	colourImage = getCamera().createCompatibleImage();
	imageWidth  = colourImage.getWidth();
	imageHeight = colourImage.getHeight();
	
	OBJECT_COLORS.addAll(Arrays.asList(BLUE, CYAN, GREEN, MAGENTA, ORANGE, RED, WHITE, YELLOW));
    }

        
    /** Initialize Agent's Behavior */
    @Override
	public void initBehavior()
    {
	setTranslationalVelocity(0.5);
    }

	private ArrayList<Color> visited = new ArrayList<Color>();
    /** 
     *
     *  ####### THIS METHOD NEEDS TO BE CHANGED BY THE STUDENT!! #######
     * 
     * Perform one step of Agent's Behavior
     *
     * Note the use of copyVisionImage and processImage
     * 
     */
    public void performRobotBehavior() {
	    processedImage = new Image();
		getCamera().copyVisionImage(colourImage);
		processImage();
	
		// Put your code here
		if (getSonars().getFrontQuadrantMeasurement() < wallClearance) {
			stopForward();
		} else {
			switch (processedImage.visibleObjects().size()) {
			case 0 : {
				stopForward();
				startRightTurn();
			} break;
			case 1 : {
				stopTurn();
				startForward();
			} break;
			default : {
				stopForward();
				startLeftTurn();
			} break;
			}
		}
	
		// When you have found all other victims, you MUST call
		// endExploration();
    }
    Image processedImage;
    private void processImage()
    {
	for (int i = 0; i < imageHeight; i++) {
	    for (int j = 0; j < imageWidth; j++) {
		// Extract the 8-bit colour values from the pixel
		int c = colourImage.getRGB(j, i);
		int red = (c & 0x00ff0000) >> 16;
		int green = (c & 0x0000ff00) >> 8;
		int blue = c & 0x000000ff;

		/*
		  You have to change to code below to detect whatever you want
		  for your convenience you should be able to use the present colors from
		  java.awt.Color:

		  java.awt.Color.BLUE;
		  java.awt.Color.CYAN;
		  java.awt.Color.GREEN;
		  java.awt.Color.MAGENTA;
		  java.awt.Color.ORANGE;
		  java.awt.Color.RED;
		  java.awt.Color.WHITE;
		  java.awt.Color.YELLOW;
		*/
		
		Color visibleColor = new Color(red, green, blue);
		processedImage.add(j, i, visibleColor);
	    }
    }
    }

    /**
     * Your subclass MUST implement this method.
     * It should traverse the map made by the robot during its
     * exploration of the arena and print out directions for how to
     * find each victim.
     *
     * You must call the methods "forwardTo()", "turnRight()", "turnLeft()"
     * to do the actual printing. This is so we can guarantee that all the
     * directions conform to our standard.
     */
    public void giveDirections()
    {
	/* put your code here */
    }
    
	////////////////////////////////////////////////////////////
	// OTHER CLASSES ///////////////////////////////////////////
	////////////////////////////////////////////////////////////
    
    private class Image extends ArrayList<ArrayList<Color>> {

		public Image() {
			super();
		}
		
		private Set<Color> visibleObjects = null;
		public Set<Color> visibleObjects() {
			if (visibleObjects == null) {
				visibleObjects = new HashSet<Color>();
				for (ArrayList<Color> a : this) {
					for (Color c : a) {
						if (OBJECT_COLORS.contains(c)) {
							visibleObjects.add(c);
						}
					}
				}
			}
			return visibleObjects;
		}

		public void add(int x, int y, Color item) {
			ArrayList<Color> row;
			if (this.size() <= y) {
				add(y, new ArrayList<Color>());
			}
			
			row = get(y);
			row.add(x, item);
		}
		
		public Color get(int x, int y) {
			return (get(y).get(x));
		}
		
		public int rows() {
			return size();
		}
		
		public int columns() {
			return get(0).size();
		}
    	
    }
    
}
